#include "RotationConversion.h"

Quaternion Rotation::MatrixToQuat(const Mat3d &mat){
	Quaternion q;
	float trace = mat[0][0] + mat[1][1] + mat[2][2] + 1.0f;

	if( trace > -EPSILON ) {
		float s = 0.5f / sqrtf(trace);
		q.w = 0.25f / s;
		q.x = ( mat[2][1] - mat[1][2] ) * s;
		q.y = ( mat[0][2] - mat[2][0] ) * s;
		q.z = ( mat[1][0] - mat[0][1] ) * s;
	} else if ( mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2] ) {
		float s = 2.0f * sqrtf( 1.0f + mat[0][0] - mat[1][1] - mat[2][2]);
		q.x = 0.25f * s;
		q.y = (mat[0][1] + mat[1][0] ) / s;
		q.z = (mat[0][2] + mat[2][0] ) / s;
		q.w = (mat[1][2] - mat[2][1] ) / s;
	} else if (mat[1][1] > mat[2][2]) {
		float s = 2.0f * sqrtf( 1.0f + mat[1][1] - mat[0][0] - mat[2][2]);
		q.x = (mat[0][1] + mat[1][0] ) / s;
		q.y = 0.25f * s;
		q.z = (mat[1][2] + mat[2][1] ) / s;
		q.w = (mat[0][2] - mat[2][0] ) / s;
	} else {
		float s = 2.0f * sqrtf( 1.0f + mat[2][2] - mat[0][0] - mat[1][1] );
		q.x = (mat[0][2] + mat[2][0] ) / s;
		q.y = (mat[1][2] + mat[2][1] ) / s;
		q.z = 0.25f * s;
		q.w = (mat[0][1] - mat[1][0] ) / s;
	}
	return q;
}

// same as above: just use the rotation part
Quaternion Rotation::MatrixToQuat(const Mat4d &mat){
	Quaternion q;
	float trace = mat[0][0] + mat[1][1] + mat[2][2] + 1.0f;

	if( trace > -EPSILON ) {
		float s = 0.5f / sqrtf(trace);
		q.w = 0.25f / s;
		q.x = ( mat[2][1] - mat[1][2] ) * s;
		q.y = ( mat[0][2] - mat[2][0] ) * s;
		q.z = ( mat[1][0] - mat[0][1] ) * s;
	} else if ( mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2] ) {
		float s = 2.0f * sqrtf( 1.0f + mat[0][0] - mat[1][1] - mat[2][2]);
		q.x = 0.25f * s;
		q.y = (mat[0][1] + mat[1][0] ) / s;
		q.z = (mat[0][2] + mat[2][0] ) / s;
		q.w = (mat[1][2] - mat[2][1] ) / s;
	} else if (mat[1][1] > mat[2][2]) {
		float s = 2.0f * sqrtf( 1.0f + mat[1][1] - mat[0][0] - mat[2][2]);
		q.x = (mat[0][1] + mat[1][0] ) / s;
		q.y = 0.25f * s;
		q.z = (mat[1][2] + mat[2][1] ) / s;
		q.w = (mat[0][2] - mat[2][0] ) / s;
	} else {
		float s = 2.0f * sqrtf( 1.0f + mat[2][2] - mat[0][0] - mat[1][1] );
		q.x = (mat[0][2] + mat[2][0] ) / s;
		q.y = (mat[1][2] + mat[2][1] ) / s;
		q.z = 0.25f * s;
		q.w = (mat[0][1] - mat[1][0] ) / s;
	}
	return q;
}

Mat3d Rotation::QuatToMatrix(Quaternion q){
	return q.getMatrix();
}

Quaternion Rotation::ExpToQuat(Vec3d v)
{
	double mag = len(v);
	if(mag > 1e-10){
		Quaternion q(v/mag, mag);
		return q;
	}
	else{
		Quaternion q(1,0,0,0);
		return q;
	}
}

Vec3d Rotation::QuatToExp(Quaternion q){
	Vec3d v = q.getAxis();
	return v*q.getAngle();
}

// Note: xyz order means matrix is Rz*Ry*Rx i.e a point as transformed as Rz*Ry*Rx(p)
// coordinate system transformation as in GL will be written as glRotate(z); glRotate(y); glRotate(x)
Vec3d Rotation::MatrixToEuler(Mat3d m, RotationOrder order)
{
	double x, y, z;

	if(order==Rotation::XYZ){
		if(m[2][0] > (1.0-EPSILON)){
			cout << "North Pole" << endl;
			x = atan2(m[0][1], m[0][2]);
			y = -M_PI / 2.0;
			z = 0.0;
		}
		if(m[2][0] < -(1.0-EPSILON)){
			cout << "South Pole" << endl;
			x = atan2(m[0][1], m[0][2]);
			y = M_PI / 2.0;
			z = 0.0;
		}
		x = atan2(m[2][1], m[2][2]);
		y = -asin(m[2][0]);
		z = atan2(m[1][0], m[0][0]);
		return Vec3d(x,y,z);	// order of return is the order of input
	}

	if(order==Rotation::ZYX){
		if(m[0][2] > (1.0-EPSILON)){
			cout << "North Pole" << endl;
			z = atan2(m[1][0], m[1][1]);
			y = M_PI / 2.0;
			x = 0.0;
		}
		if(m[0][2] < -(1.0-EPSILON)){
			cout << "South Pole" << endl;
			z = atan2(m[1][0], m[1][1]);
			y = -M_PI / 2.0;
			x = 0.0;
		}
		z = -atan2(m[0][1], m[0][0]);
		y = asin(m[0][2]);
		x = -atan2(m[1][2], m[2][2]);
		return Vec3d(z,y,x);	// order of return is the order of input
	}

	if(order=Rotation::YZX){
		if(m[0][1] > (1.0-EPSILON)){
			cout << "North Pole" << endl;
			y = atan2(m[1][2], m[1][0]);
			z = -M_PI / 2.0;
			x = 0.0;
		}
		if(m[0][1] < -(1.0-EPSILON)){
			cout << "South Pole" << endl;
			y = atan2(m[1][2], m[1][0]);
			z = M_PI / 2.0;
			x = 0.0;
		}
		y = atan2(m[0][2], m[0][0]);
		z = -asin(m[0][1]);
		x = atan2(m[2][1], m[1][1]);
		return Vec3d(y,z,x);	// order of return is the order of input
	}

	if(order==Rotation::XZY){
		if(m[1][0] > (1.0-EPSILON)){
			cout << "North Pole" << endl;
			x = -atan2(m[0][2], m[0][1]);
			z = M_PI / 2.0;
			y = 0.0;
		}
		if(m[1][0] < -(1.0-EPSILON)){
			cout << "South Pole" << endl;
			x = -atan2(m[0][2], m[0][1]);
			z = -M_PI / 2.0;
			y = 0.0;
		}
		x = -atan2(m[1][2], m[1][1]);
		z = asin(m[1][0]);
		y = -atan2(m[2][0], m[0][0]);
		return Vec3d(x,z,y);	// order of return is the order of input
	}

	if(order==Rotation::YXZ){
		if(m[2][1] > (1.0-EPSILON)){
			cout << "North Pole" << endl;
			y = atan2(m[0][2], m[0][0]);
			x = M_PI / 2.0;
			z = 0.0;
		}
		if(m[2][1] < -(1.0-EPSILON)){
			cout << "South Pole" << endl;
			y = atan2(m[0][2], m[0][0]);
			x = -M_PI / 2.0;
			z = 0.0;
		}
		y = -atan2(m[2][0], m[2][2]);
		x = asin(m[2][1]);
		z = -atan2(m[0][1], m[1][1]);
		return Vec3d(y, x, z);	// order of return is the order of input
	}

	if(order==Rotation::ZXY){
		if(m[1][2] > (1.0-EPSILON)){
			cout << "North Pole" << endl;
			z = -atan2(m[0][1], m[0][0]);
			x = -M_PI / 2.0;
			y = 0.0;
		}
		if(m[1][2] < -(1.0-EPSILON)){
			cout << "South Pole" << endl;
			z = -atan2(m[0][1], m[0][0]);
			x = M_PI / 2.0;
			y = 0.0;
		}
		z = atan2(m[1][0], m[1][1]);
		x = -asin(m[1][2]);
		y = atan2(m[0][2], m[2][2]);
		return Vec3d(z,x,y);	// order of return is the order of input
	}
	printf("MatrixToEuler - Do not support rotation order %s. Make sure letters are in lowercase\n", order);
	return vl_0;
}

Mat3d Rotation::EulerToMatrix(Vec3d v, RotationOrder order){
	if(order==Rotation::XYZ){
		return EulerToMatrixZ(v[2])*EulerToMatrixY(v[1])*EulerToMatrixX(v[0]);
	}
	if(order==Rotation::ZYX){
		return EulerToMatrixX(v[2])*EulerToMatrixY(v[1])*EulerToMatrixZ(v[0]);
	}
	if(order==Rotation::YZX){
		return EulerToMatrixX(v[2])*EulerToMatrixZ(v[1])*EulerToMatrixY(v[0]);
	}
	if(order==Rotation::XZY){
		return EulerToMatrixY(v[2])*EulerToMatrixZ(v[1])*EulerToMatrixX(v[0]);
	}
	if(order==Rotation::YXZ){
		return EulerToMatrixZ(v[2])*EulerToMatrixX(v[1])*EulerToMatrixY(v[0]);
	}
	if(order==Rotation::ZXY){
		return EulerToMatrixY(v[2])*EulerToMatrixX(v[1])*EulerToMatrixZ(v[0]);
	}

	printf("EulerToMatrix - Do not support rotation order %s. Make sure letters are in lowercase\n", order);
	return vl_1;
}

Mat3d Rotation::EulerToMatrixX(double x){
	Mat3d mat(vl_0);
	double cosangle = cos(x);
	double sinangle = sin(x);
	mat[0][0] = 1.0; 
	mat[1][1] = cosangle; 
	mat[1][2] = -sinangle; 
	mat[2][1] = sinangle; 
	mat[2][2] = cosangle; 
	return mat;
}

Mat3d Rotation::EulerToMatrixY(double y){
	Mat3d mat(vl_0);
	double cosangle = cos(y);
	double sinangle = sin(y);
	mat[1][1] = 1.0; 
	mat[2][2] = cosangle; 
	mat[2][0] = -sinangle; 
	mat[0][2] = sinangle; 
	mat[0][0] = cosangle; 
	return mat;
}

Mat3d Rotation::EulerToMatrixZ(double z){
	Mat3d mat(vl_0);
	double cosangle = cos(z);
	double sinangle = sin(z);
	mat[2][2] = 1.0; 
	mat[0][0] = cosangle; 
	mat[0][1] = -sinangle; 
	mat[1][0] = sinangle; 
	mat[1][1] = cosangle; 
	return mat;
}
